#ifndef PNG_PCD_PATH_HPP
#define PNG_PCD_PATH_HPP

#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <sstream>
#include <map>
#include <yaml-cpp/yaml.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/pca.h>
#include <Eigen/Dense>




#include <nlohmann/json.hpp>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <ctime>



/**
 * @brief 算法参数结构体
 */
struct AlgorithmParameters
{
    // 直通滤波器参数
    struct PassThroughFilter {
        float min_z = -114.0f;
        float max_z = -60.0f;
    } passthrough_filter;
    
    // 体素滤波器参数
    struct VoxelFilter {
        float voxel_size = 1.5f;
    } voxel_filter;
    
    // kNN滤波参数
    struct KNNFilter {
        int k = 10;
        double distance_threshold = 4.0;
    } knn_filter;
    
    // 欧式聚类参数
    struct EuclideanClustering {
        double cluster_tolerance = 2.0;
        int min_cluster_size = 100;
        int max_cluster_size = 2500000;
    } euclidean_clustering;
    
    // 边缘提取参数
    struct BoundaryExtraction {
        float step_x = 1.5f;                    // X方向扫描步长
        float scanline_x_range_factor = 0.5f;   // 扫描线X方向采样范围因子（相对于step_x的比例）
        double min_edge_distance = 3.0;        // 两个边缘点间最小欧式距离
        bool enable_distance_filter = true;     // 是否启用距离过滤
    } boundary_extraction;
    
    // 边缘点聚类和Z高度统计滤波参数
    struct EdgeKNNFilter {
        double radius = 8.0;           // Z高度统计滤波搜索半径
        int min_neighbors = 3;         // 最小邻居点数
        double outlier_threshold = 2.0; // 异常值阈值（标准差倍数）
        bool enable_filter = true;     // 是否启用滤波
        // 聚类滤波参数
        bool enable_cluster_filter = true;    // 是否启用聚类滤波
        double cluster_tolerance = 3.0;       // 聚类容忍度
        int min_cluster_size = 5;             // 最小聚类大小
        int max_cluster_size = 1000;          // 最大聚类大小
    } edge_knn_filter;
    
    // 边缘点平滑滤波和插值参数
    struct EdgeSmoothing {
        // 平滑滤波参数
        bool enable_smooth_filter = true;      // 是否启用平滑滤波
        int smooth_neighbor_count = 5;         // 平滑邻域点数量
        double smooth_weight_sigma = 2.0;      // 高斯权重标准差
        
        // // 插值参数
        // bool enable_interpolation = true;      // 是否启用插值
        // double max_gap_distance = 15.0;        // 最大间隙距离，超过此距离需要插值
        // double interpolation_step = 3.0;       // 插值步长
        // int max_interpolation_points = 10;     // 单个间隙最大插值点数
    } edge_smoothing;
    
    // 法向量计算参数
    struct NormalComputation {
        double neighbor_radius = 10.0;
        int min_neighbors = 10;
        // 法向量角度滤波参数
        bool enable_normal_smoothing = true;    // 是否启用法向量平滑
        double smooth_radius = 8.0;             // 法向量平滑半径
        double angle_threshold = 30.0;          // 法向量角度阈值（度）
        int smooth_iterations = 2;              // 平滑迭代次数
    } normal_computation;
    
    // 内侧打胶点生成参数
    struct InnerGluingPoints {
        double offset_distance = 18.0;
        bool use_projection_height = true;
        double search_radius = 15.0;            // 搜索半径，用于查找最高点
        double height_offset = 8.0;             // 在最高点基础上的高度偏移量
        // Z高度平滑处理参数
        bool enable_z_smoothing = true;         // 是否启用Z高度平滑处理
        double z_smooth_radius = 8.0;           // Z高度平滑搜索半径
        int z_smooth_iterations = 2;            // Z高度平滑迭代次数
        // Z高度异常点过滤参数
        bool enable_z_outlier_filter = true;    // 是否启用Z高度异常点过滤
        double z_outlier_threshold = 2.5;       // Z高度异常点阈值（标准差倍数）
        double z_filter_radius = 12.0;          // 异常点检测搜索半径
    } inner_gluing_points;
    
    // 外部喷胶偏移调节参数
    struct OuterSprayOffset {
        bool enable_offset_adjustment = true;   // 是否启用外部喷胶偏移调节
        bool is_left_foot = true;               // 是否为左脚（影响分区方向）
        
        // X方向5个分区的百分比参数（各分区占用的百分比，总和为100%）
        double x_region_percent_1 = 15.0;          // 第1个分区占用百分比 (15%)
        double x_region_percent_2 = 20.0;          // 第2个分区占用百分比 (20%)
        double x_region_percent_3 = 30.0;          // 第3个分区占用百分比 (30%)
        double x_region_percent_4 = 25.0;          // 第4个分区占用百分比 (25%)
        double x_region_percent_5 = 10.0;          // 第5个分区占用百分比 (10%)
        
        // 连通性检测参数
        double connection_gap_threshold = 20.0; // 连通性间隙阈值，超过此距离认为是断开的路径
        
        // 8个区域的Z高度调节参数（扩展为8个路径线）
        double z_offset_region_1 = 0.0;
        double z_offset_region_2 = 0.0;
        double z_offset_region_3 = 0.0;
        double z_offset_region_4 = 0.0;
        double z_offset_region_5 = 0.0;
        double z_offset_region_6 = 0.0;
        double z_offset_region_7 = 0.0;
        double z_offset_region_8 = 0.0;
        // 8个区域的按法向量内移调节参数
        double normal_offset_region_1 = 0.0;
        double normal_offset_region_2 = 0.0;
        double normal_offset_region_3 = 0.0;
        double normal_offset_region_4 = 0.0;
        double normal_offset_region_5 = 0.0;
        double normal_offset_region_6 = 0.0;
        double normal_offset_region_7 = 0.0;
        double normal_offset_region_8 = 0.0;
        // 8个分区的绕Y轴偏移角（度）
        double axis_angle_1 = 0.0;
        double axis_angle_2 = 0.0;
        double axis_angle_3 = 0.0;
        double axis_angle_4 = 0.0;
        double axis_angle_5 = 0.0;
        double axis_angle_6 = 0.0;
        double axis_angle_7 = 0.0;
        double axis_angle_8 = 0.0;
    } outer_spray_offset;
    
    // 路径采样参数
    struct PathSampling {
        int outer_points = 40;
        int inner_points = 30;
    } path_sampling;

    // 路径高度平滑参数（针对采样后的路径点）
    struct PathSmoothing {
        bool enable_z_smoothing = true;    // 是否对采样后的路径做Z高度平滑
        double z_smooth_radius = 8.0;      // 平滑邻域搜索半径
        int z_smooth_iterations = 2;       // 平滑迭代次数
        double z_outlier_threshold = 2.5;  // 判定为异常点的阈值（标准差倍数）
    } path_smoothing;

    // 路径半径滤波参数（当半径内存在多个路径点时，剔除与鞋面质心更远的点）
    struct PathRadiusFilter {
        bool enable_radius_filter = true;  // 是否启用路径半径滤波
        double radius = 6.0;               // 半径阈值
    } path_radius_filter;
};






/**
 * @brief 鞋底打胶路径算法处理类
 */
class ShoeSolePathAlgorithm
{
private:
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_;      // 输入点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_;   // 滤波后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr edge_cloud_;       // 边缘点云
    std::vector<pcl::Normal> edge_normals_;                // 边缘点法向量
    pcl::PointCloud<pcl::PointXYZ>::Ptr inner_gluing_points_; // 内侧打胶点
    pcl::PointCloud<pcl::PointXYZI>::Ptr outer_gluing_points_; // 外侧打胶点（带强度值用于区域可视化）

    // 采样和变换后的路径
    pcl::PointCloud<pcl::PointXYZI>::Ptr sampled_outer_path_;   // 采样后的外部路径（带强度值用于颜色编码）
    pcl::PointCloud<pcl::PointXYZ>::Ptr sampled_inner_path_;   // 采样后的内部路径
    pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_outer_path_; // 变换后的外部路径（带强度值用于颜色编码）
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_inner_path_; // 变换后的内部路径
    
    // 新增：使用vector存储带姿态信息的路径 [x,y,z,rx,ry,rz]
    std::vector<std::vector<float>> outer_path_poses_;           // 外侧路径姿态信息（采样后）
    std::vector<std::vector<float>> inner_path_poses_;           // 内侧路径姿态信息（采样后）
    std::vector<std::vector<float>> transformed_outer_path_poses_; // 变换后外侧路径姿态信息
    std::vector<std::vector<float>> transformed_inner_path_poses_; // 变换后内侧路径姿态信息
    
    // 新增：全局清扫路径（合并外部和内部路径）
    std::vector<std::vector<float>> global_sweep_path_poses_;    // 全局清扫路径姿态信息（排序后合并）
    std::vector<std::vector<float>> transformed_global_sweep_path_poses_; // 变换后全局清扫路径姿态信息 
    
    // 标定变换矩阵
    Eigen::Matrix4f calib_matrix_;

    // 算法参数
    AlgorithmParameters params_;

    // kNN滤波参数（保持向后兼容）
    int knn_k_;              // k近邻数量，默认为20
    double distance_thresh_; // 距离阈值，默认为1.5

public:

    ShoeSolePathAlgorithm();
    ~ShoeSolePathAlgorithm();
    
    // 参数管理
    bool loadParametersFromFile(const std::string& param_file = "param.yaml");
    void printParameters() const;
    const AlgorithmParameters& getParams() const { return params_; }
    
    bool loadPointCloud(const std::string& filename);
    
    // 使用参数文件的版本（推荐）
    bool applyPassThroughFilter();
    bool applyVoxelFilter();
    bool applyKNNFilter();
    bool applyEuclideanClustering();
    bool extractSoleBoundary();
    bool applyEdgeKNNFilter();  // 新增：边缘点基于Z高度的统计滤波
    bool applyEdgeSmoothing();  // 新增：边缘点平滑滤波和插值处理
    bool computeEdgeNormals();
    bool generateInnerGluingPoints();
    bool uniformSamplePaths();
    bool transformSampledPaths();
    
    // 兼容旧版本的函数（明确带参数版本） - 已整合到主函数中
    pcl::PointCloud<pcl::PointXYZ>::Ptr getFilteredCloud() const;
    pcl::PointCloud<pcl::PointXYZ>::Ptr getEdgeCloud() const;
    std::vector<pcl::Normal> getEdgeNormals() const;
    bool saveEdgePathWithNormalsPCD(const std::string& filename);
    bool saveInnerGluingPointsPCD(const std::string& filename);
    
    // 内侧打胶点Z高度处理函数
    void smoothInnerGluingPointsZ();                      // Z高度平滑处理
    void filterInnerGluingPointsOutliers();              // Z高度异常点过滤
    // 新增：对采样后路径的高度平滑与半径滤波
    void smoothSampledPaths();                            // 对采样后的内外路径做Z高度平滑（考虑异常值）
    void radiusFilterSampledPaths();                      // 对采样后的路径做半径滤波
    
    // 新增：改进的路径处理功能
    bool reorganizeOuterContourPath();  // 将整个外轮廓重新按要求排序
    std::vector<std::vector<size_t>> dividePathByXRegions();  // 按X区域将路径划分为5个部分
    std::vector<std::vector<size_t>> splitAndSortPathSegments(const std::vector<std::vector<size_t>>& x_regions);  // 拆分和排序路径段
    bool generateOuterGluingPoints();  // 生成外侧打胶点
    bool saveOuterGluingPointsPCD(const std::string& filename);  // 保存外侧打胶点
    bool isPointConnectedToNext(size_t current_index, size_t next_index) const;  // 检查两个点是否连通

    // 新增：路径采样和变换功能
    // 新增：对采样路径计算角度并保存至内/外路径（intensity字段保存角度，弧度）
    void processPathOrientations();
    // 新增：创建全局清扫路径（合并外部和内部路径）
    void createGlobalSweepPath();
    // 新增：保存带角度的采样路径到TXT文件（x y z angle）
    // 新增：保存全局清扫路径到TXT文件
    bool saveGlobalSweepPath(const std::string& global_sweep_txt_filename);
    bool saveSampledPaths(const std::string& outer_filename, const std::string& inner_filename);
    
    bool saveTransformedPaths(const std::string& outer_pcd_filename, 
                              const std::string& inner_pcd_filename,
                              const std::string& outer_txt_filename, 
                              const std::string& inner_txt_filename);

    // 通用保存函数
    bool saveFilteredCloud(const std::string& filename);
    bool saveEdgePath(const std::string& filename);

    void printStatistics();
    void socket_send_code();
private:
    // 整合版本支持的private重载函数（保持API兼容性）
    bool applyPassThroughFilter(float min_z, float max_z);
    bool applyVoxelFilter(float voxel_size);
    bool applyKNNFilter(int k, double distance_threshold);
    bool applyEuclideanClustering(double cluster_tolerance, int min_cluster_size, int max_cluster_size);
    bool extractSoleBoundary(float step_x);
    bool computeEdgeNormals(double neighbor_radius, int min_neighbors);
    bool generateInnerGluingPoints(double offset_distance, bool use_projection_height);
    bool uniformSamplePaths(int outer_points, int inner_points);
    
    double computeKthNearestDistance(const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree, 
                                   const pcl::PointXYZ& point, int k);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sortPointsByX(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sortPointsByY(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
    pcl::PointXYZ findExtremePoint(const std::vector<pcl::PointXYZ>& points,
                                 bool find_max, bool from_top);

    pcl::Normal computePointNormal(const pcl::PointXYZ& point,
                                  const std::vector<pcl::PointXYZ>& neighbors,
                                  const Eigen::Vector3f& shoe_centroid);

    // 法向量平滑处理
    void applyNormalSmoothing();
    void enforceOutwardNormalDirection();                 // 确保法向量指向外侧
    pcl::Normal smoothNormalWithNeighbors(size_t point_index, const std::vector<size_t>& neighbor_indices, 
                                         const std::vector<pcl::Normal>& input_normals);

    // 角度计算相关函数
    Eigen::Vector3f calculateInnerPathOrientation(const pcl::PointXYZ& point, size_t index);  // 计算内侧路径点的方向角（x朝下，y朝左）
    Eigen::Vector3f calculateOuterPathOrientation(const pcl::PointXYZI& point, size_t index);  // 计算外侧路径点的方向角（x沿法向量，z朝上）

    // 路径排序相关函数
    std::vector<std::vector<float>> sortPathByAngle(const std::vector<std::vector<float>>& path_poses);

    size_t findMaxXPointIndex(const std::vector<std::vector<float>>& path_poses);  // 注意：现在查找最小X值
    
    
    // 内侧点投影和高度计算
    pcl::PointXYZ projectPointToFilteredCloud(const pcl::PointXYZ& point);
    double findMaxHeightAroundProjection(const pcl::PointXYZ& projected_point);
    

    double basisFunction(int i, int degree, double t, const std::vector<double>& knots);
    std::vector<double> generateKnotVector(int num_control_points, int degree);
    
    // 基于距离的均匀采样辅助函数
    std::vector<double> computeCumulativeDistances(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud);
    std::vector<double> computeCumulativeDistances(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
    size_t findPointByDistance(const std::vector<double>& cumulative_distances, double target_distance);
    
    // 点云路径排序函数（将无序点云排序为连续路径）
    pcl::PointCloud<pcl::PointXYZI>::Ptr sortPointCloudAsPath(const pcl::PointCloud<pcl::PointXYZI>::Ptr& input);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sortPointCloudAsPath(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input);
    
};

#endif // PNG_PCD_PATH_HPP
